
#include <VirtualRobot/RuntimeEnvironment.h>
#include <VirtualRobot/Import/RobotImporterFactory.h>

#include <Inventor/Qt/SoQt.h>
#include <QFileInfo>

#include <boost/extension/shared_library.hpp>
#include <boost/function.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/filesystem.hpp>
#include <boost/foreach.hpp>
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string.hpp>
#include <string>
#include <map>
#include <iostream>
#include <fstream>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <MMM/Motion/Legacy/LegacyMotionReaderXML.h>
#include <MMM/Motion/Legacy/LegacyMotionReaderC3D.h>
#include <MMM/Model/ModelReaderXML.h>
#include <MMM/Model/ModelProcessor.h>
#include <MMM/Model/ModelProcessorFactory.h>
#include <MMM/Motion/Legacy/Converter/ConverterFactory.h>

//#include "MMMConverterConfiguration.h"

using std::cout;
using std::endl;
using namespace VirtualRobot;

int main(int argc, char *argv[])
{
    cout << " --- MMM motion merger --- " << endl;
    cout << "merges two mmm motion files into one motion file containing both motions." << endl;

    // we are expecting three arguments, and that is the filename of the file to convert
    if (argc<4)
    {
        cout << "Missing filename as first argument, aborting..." << endl;
        cout << "Syntax: mergemmm %motionfile1_in %motionfile2_in %targetmotionfile_out" << endl;
        //MMM_ERROR << endl << "Could not process command line, aborting..." << endl;
        return -1;
    }
    //c.print();


    std::string filename1 = std::string(argv[1]);
    std::string filename2 = std::string(argv[2]);
    std::string filenameOut = std::string(argv[3]);

    cout << "Filenames provided for conversion: [" << filename1 << "] + [" << filename2 << "] -> [" << filenameOut << "]" << endl;

    if (!filename1.empty() && !filename2.empty())
    {
        // input file 1
        QFileInfo fileInfo1(filename1.c_str());
        std::string path1(fileInfo1.path().toLatin1());
        std::string file1(fileInfo1.fileName().toLatin1());
        std::string suffix1(fileInfo1.suffix().toLatin1());
        // input file 2
        QFileInfo fileInfo2(filename2.c_str());
        std::string path2(fileInfo2.path().toLatin1());
        std::string file2(fileInfo2.fileName().toLatin1());
        std::string suffix2(fileInfo2.suffix().toLatin1());
        // transform suffixes to lowerCase
        std::transform(suffix1.begin(), suffix1.end(), suffix1.begin(), ::tolower);
        std::transform(suffix2.begin(), suffix2.end(), suffix2.begin(), ::tolower);

// #ifdef false
//         cout << "Path to Files are [" << path1 << "] and ["  << path2 << "]" << endl;
//         cout << "Filenames are [" << file1 << "] and [" << file2 << "]" << endl;
//         cout << "Suffixes are [" << suffix1 << "] and [" << suffix2 << "]" << endl;
//         cout << "Resulting motion will be saved as [" << newFilename << "]" << endl;
// #endif

        // For now, we just concatenate MMM motion files, so we check for correct suffixes
        if ((suffix1.compare("xml")!=0) || (suffix2.compare("xml")!=0))
        {
            cout << "Expected MMM Motion files as input (.xml). Aborting..." << endl;
            return -1;
        }
        // load input files
        MMM::LegacyMotionReaderXMLPtr r(new MMM::LegacyMotionReaderXML());
        cout << "Checking motions in file 1: " << endl;
        std::vector < std::string > motionNames1 = r->getMotionNames(filename1);
        cout << "Checking motions in file 2: " << endl;
        std::vector < std::string > motionNames2 = r->getMotionNames(filename2);
        // there should be at least one motion in every file
        if (motionNames1.size() == 0)
        {
            MMM_ERROR << "no motions in file 1!" << endl;
            return -1;
        }
        else
            cout << "Number of Motions in file1:" << motionNames1.size() << endl;
        if (motionNames2.size() == 0)
        {
            MMM_ERROR << "no motions in file 2!" << endl;
            return -1;
        }
        else
            cout << "Number of Motions in file2:" << motionNames2.size() << endl;
        // save maximum frame numbers over all motions in this integer
        int frameCount = -1;
        // save motions in a motion list for faster access
        MMM::LegacyMotionList motions;
        // find the maximum frame number by iterating over all motions
        for(size_t i = 0; i < motionNames1.size(); i++)
        {
            MMM::LegacyMotionPtr motion = r->loadMotion(filename1,motionNames1[i]);
            // skip empty motions
            if(!motion)
            {
                cout << "skipping motion [" << motionNames1[i] << "]" << endl;
                continue;
            }
            motions.push_back(motion);
            cout << "Checking motion: [" << motion->getName() << "]";
            if(frameCount == -1 || frameCount < (int) motion->getNumFrames())
            {
                frameCount = motion->getNumFrames();
                cout << "\tSetting maximum frameCount to " << frameCount << endl;
            }
            cout << "... Number of frames: [" << motion->getNumFrames() << "]" << endl;
            cout  << "\tcontaining model file: [" << (motion->getModel() ? motion->getModel()->getFilename() : "unknown") << "]" << endl;
        }
        for(size_t i = 0; i < motionNames2.size(); i++)
        {
            MMM::LegacyMotionPtr motion = r->loadMotion(filename2,motionNames2[i]);
            // skip empty motions
            if(!motion)
            {
                cout << "skipping motion [" << motionNames2[i] << "]" << endl;
                continue;
            }
            motions.push_back(motion);
            cout << "Checking motion: [" << motion->getName() << "]";
            if(frameCount == -1 || frameCount < (int) motion->getNumFrames())
            {
                frameCount = motion->getNumFrames();
                cout << "\tSetting maximum frameCount to " << frameCount << endl;
            }
            cout << "... Number of frames: [" << motion->getNumFrames() << "]" << endl;
            cout  << "\tcontaining model file: [" << (motion->getModel() ? motion->getModel()->getFilename() : "unknown") << "]" << endl;
        }
        cout << "Number of total frames: [" << frameCount << "]" << endl;
        cout << "Number of total motions: [" << motions.size() << "]" << endl;
        // Expand every motion to the max frame size
        for (unsigned int i=0; i<motions.size(); i++)
        {
            MMM::LegacyMotionPtr motion = motions[i];
            // calculate missing motion frames
            int delta = frameCount - motion->getNumFrames();
            // if no frames are missing, continue with next motion
            if (delta == 0)
                continue;
            // otherwise duplicate the last frame j-times, so that the frame count matches
            MMM::MotionFramePtr frame = motion->getMotionFrame(motion->getNumFrames()-1);
            // predefined timestep
            float dt = 0.01f;
            float time = frame->timestep;
            cout << "adding [" << delta << "] frames to the motion [" << motion->getName() << "]" << " using frame at timestep ["<< time << "]" << endl;
            // if there are at least 2 motion frames, calculate the timestep delta, otherwise use the predefined value
            if (motion->getNumFrames()>1)
            {
                dt = motion->getMotionFrame(1)->timestep - motion->getMotionFrame(0)->timestep;
                cout << "\tcalculated delta-time: [" << dt << "]" << endl;
            } else
                cout << "\tpre-defined delta-time: [" << dt << "]" << endl;
            MMM::MotionFramePtr f;
            for (int j=1; j<delta+1; j++)
            {
                // make a copy of frame last frame
                f.reset(new MMM::MotionFrame(*frame));
                // and only change the timestep property
                f->timestep = time + dt*j;
                // add frame to motion
                motion->addMotionFrame(f);
            }
        }
        // xml-string for all motions
        std::string content;
        // xml-string for each motion
        std::string contentMotion;
        // concatenate the xml-strings from every motion to a single xml-string
        for (unsigned int i=0; i<motions.size(); i++)
        {
            contentMotion = motions[i]->toXML();
            content += contentMotion;
        }
        // save this xml-string to the output file
        if (!MMM::XML::saveXML(filenameOut, content))
        {
            MMM_ERROR << " Could not write to file " << filenameOut << endl;
        }
    }
    return 0;
}
